#include <avr/interrupt.h>
#include <stdlib.h>
#include <avr/io.h>
#include <string.h>

#include "common.h"
#include "app.h"
#include "led.h"
#include "radio\radio.h"
#include "uart\uart.h"


void init_global(){

    /** Disable default prescaler to make processor speed 8 MHz. */
    CLKPR = (1<<CLKPCE);
    CLKPR = 0x00;	


	//BEGIN Servo Motor
	DDRC |= (1<<PORTC5); //Enable output for PSWM

	TCCR3B = (0<<CS30) | (1<<CS31) | (0<<CS32) | (1<<WGM33) | (1<<WGM32); 
	TCCR3A = (0<<WGM30) | (1<<WGM31) | (1<<COM3B1) | (0<<COM3B0);

	ICR3 = 40000; // The max width for the PWM (TOP)
	OCR3B = duty_closed;
	//END Servo Motor

	//BEGIN IR-SENSOR
	PORTE &= ~(PORTE1 | PORTE3);
	//END IR-SENSOR

	//BEGIN LED
	init_LED_D2();
	init_LED_D5();
	//END LED


	//BEGIN RADIO
	radio_init((uint8_t) 115, 0xABBA, 1);
	
	if (LOCK_NUM=='1'){
		uart_init();
		uart_write("Connected\r\n", 11);
	}	
	
	//END RADIO
	sei(); /** enable global interrupts **/	
	radio_print("Gate %c: Connected\r\n", LOCK_NUM);	
}

volatile uint8_t radio_cmd;
STATES state=LOCK_DISABLED;

int main(){
	
	uint8_t cur_cmd;
	OUTPUTS output=0;
	INPUTS  input=0;
	char next_lock, prev_lock;
	//char state_msg[10]="state: \r\n";
	//char cmd_msg[9]="<01>";

	init_global();

	OCR3B = duty_closed;

	if (LOCK_NUM=='1'){
		state=LOCK_CLOSED;
	}
	next_lock=LOCK_NUM+1;
	if (LOCK_NUM==LOCK_COUNT){
		next_lock='1';
	}
	prev_lock=LOCK_NUM-1;
	if (LOCK_NUM=='1'){
		prev_lock='2';
	}

	//itoa(state, &state_msg[6], 10);
	//radio_print(state_msg);

	for(;;){
		output=0;
		input=0;
		//state_msg[6]
		if(LOCK_NUM==LOCK_COUNT){
			radio_print("\t\t\t\tGate %c: state=%d \r\n", LOCK_NUM, state);			
		}else{
			radio_print("Gate %c: state=%d \r\n", LOCK_NUM, state);
		}
		
		off_LED(LED_D5_RED | LED_D5_GREEN);
		
		if (state==LOCK_CLOSED){
			on_LED(LED_D2_RED);
			off_LED(LED_D2_GREEN);

			if( PINE & _BV(PORTE3) )	// pin is high - no interruption
			{
				off_LED(LED_D5_RED);
				on_LED(LED_D5_GREEN);				
			}else{
				off_LED(LED_D5_GREEN);
				on_LED(LED_D5_RED);				
				input=S1_INTERRUPTED;
			
			}

		}else if (state==LOCK_OPENED){
			
			on_LED(LED_D2_GREEN);
			off_LED(LED_D2_RED);

			if((PINE & (_BV(PORTE1))) )	// pin is high - no interruption
			{
				on_LED(LED_D5_GREEN);
				off_LED(LED_D5_RED);
			}else{
				off_LED(LED_D5_GREEN);
				on_LED(LED_D5_RED);
				input=S2_INTERRUPTED;			
			}
			
		}else if (state==LOCK_DISABLED || state==LOCK_WAITING){
			if (state==LOCK_DISABLED){
				off_LED(LED_D2_GREEN | LED_D2_RED);
			}else{
				on_LED(LED_D2_GREEN | LED_D2_RED);
			}
			

			cur_cmd=radio_cmd;
			if (cur_cmd=='1'){
				input=R_CMD_ENABLE;
			}else if (cur_cmd=='2'){
				input=R_CMD_FEEDBACK;
			}
			radio_cmd=' ';
		}
		
		
		switch(state){
			case LOCK_DISABLED:
				if (input==R_CMD_ENABLE){
					output=ENABLE_LOCK;

					radio_print("<%c2>\r\n", prev_lock); //send twice
					delayFor(10);
					radio_print("<%c2>\r\n", prev_lock); //send twice
					delayFor(10);
					radio_print("<%c2>\r\n", prev_lock); //send twice
				}
				break;
			case LOCK_CLOSED:
				if (input==S1_INTERRUPTED){
					output=OPEN_LOCK;
				}
				break;
			case LOCK_OPENED:
				if (input==S2_INTERRUPTED){
					output=CLOSE_LOCK;
				}
				break;
			case LOCK_WAITING:
				if (input!=R_CMD_FEEDBACK){
					output=SEND_MSG;
				}else{
					state=LOCK_DISABLED;
				}
				break;
		}

	
		switch (output){
			case SEND_MSG:
			case CLOSE_LOCK:
				if(LOCK_NUM==LOCK_COUNT){
					radio_print("<%c1>\r\n", next_lock); //send twice
				}else{
					radio_print("<%c1>\r\n", next_lock); //send twice
				}
				break;
			default:
				break;
		}

		switch (output){
			case ENABLE_LOCK:
				state=LOCK_CLOSED;
				break;
			case OPEN_LOCK:
				OCR3B = duty_opened;
				state=LOCK_OPENED;
				break;
			case CLOSE_LOCK:
				OCR3B = duty_closed;
				state=LOCK_WAITING;
				break;
			default:
				break;
		}
		
		delayFor(1000);
	}
	
	return 1;

}

static uint8_t radio_buf[PAYLOAD_BYTES];

/**
 * Interrupt service routine for when data arrives at the radio (incoming).
 */
ISR (INT4_vect)
{
    int i;
    int still_good;
    int len;

	uint8_t* buf_ptr;
	uint8_t prevByte, curByte;	


    still_good = 1;
    len = 0;
    buf_ptr = radio_buf;

	
    // Dump the radio to the buffer
    for(i=0; i < PAYLOAD_BYTES; ++i)
    {
		curByte= radio_get_byte();
        *buf_ptr = curByte;
        if(still_good && *buf_ptr == 0)
        {
            len = i;
            still_good = 0;
        }

		prevByte=curByte;
        ++buf_ptr; 
    }
    if(still_good) len = PAYLOAD_BYTES;

	if (len>3){
		if (radio_buf[0]=='<' && radio_buf[3]=='>'){
			if (radio_buf[1]==LOCK_NUM){
				//PORTD ^= LED_D2_GREEN;
				radio_cmd=radio_buf[2];
			}
		}
	}

    // Write the string up to first nul
	if (LOCK_NUM=='1'){
    	uart_write(radio_buf, len);
	}

}
